#include <jtag/jtag_tap.h>

/* A reserved manufacturer ID is used in END_OF_CHAIN_FLAG, so we
 * know that no valid TAP will have it as an IDCODE value.
 */
#define END_OF_CHAIN_FLAG       0xffffffff


static void jtag_add_scan_check(jtag_tap *active, void (*jtag_add_scan)(jtag_tap *active, int in_num_fields, const struct scan_field *in_fields, tap_state_t state),
								int in_num_fields, struct scan_field *in_fields, tap_state_t state)
{
	jtag_add_scan(active, in_num_fields, in_fields, state);
}

static void jtag_add_ir_scan_noverify_callback(jtag_tap *active,
											   int dummy,
											   const struct scan_field *in_fields,
											   tap_state_t state)
{
	active->add_ir_scan_noverify(in_fields, state);
}

void jtag_tap::jtag_checks(void)
{
	assert(jtag_trst == 0);
}

void jtag_tap::jtag_prelude(tap_state_t state)
{
	jtag_checks();

	assert(state != TAP_INVALID);

	cmd_queue_cur_state = state;
}

jtag_tap::jtag_tap()
{
	_adapter = 0;
	jtag_verify = true;
	jtag_verify_capture_ir = true;
	jtag_trst = -1;
	jtag_srst = -1;
	jtag_error = ERROR_OK;
	cmd_queue_cur_state = TAP_RESET;
	jtag_ntrst_assert_width = 0;
	jtag_ntrst_delay = 0;
	jtag_reset_config = RESET_HAS_SRST;
}

void jtag_tap::bind(openocd::adapter *ad)
{
	_adapter = ad;
}

int jtag_tap::init(void)
{
	int retval;

	add_reset(0, 0);

	retval = execute_queue();
	if (retval != ERROR_OK)
		return retval;

	return ERROR_OK;
}

void jtag_tap::add_reset(int req_tlr_or_trst, int req_srst)
{
    int trst_with_tlr = 0;
    int new_srst = 0;
    int new_trst = 0;

    /* Without SRST, we must use target-specific JTAG operations
     * on each target; callers should not be requesting SRST when
     * that signal doesn't exist.
     *
     * RESET_SRST_PULLS_TRST is a board or chip level quirk, which
     * can kick in even if the JTAG adapter can't drive TRST.
     */
    if (req_srst) {
        if (!(jtag_reset_config & RESET_HAS_SRST)) {
            LOG_ERROR("BUG: can't assert SRST");
            set_error(ERROR_FAIL);
            return;
        }
        if ((jtag_reset_config & RESET_SRST_PULLS_TRST) != 0
                && !req_tlr_or_trst) {
            LOG_ERROR("BUG: can't assert only SRST");
            set_error(ERROR_FAIL);
            return;
        }
        new_srst = 1;
    }

    /* JTAG reset (entry to TAP_RESET state) can always be achieved
     * using TCK and TMS; that may go through a TAP_{IR,DR}UPDATE
     * state first.  TRST accelerates it, and bypasses those states.
     *
     * RESET_TRST_PULLS_SRST is a board or chip level quirk, which
     * can kick in even if the JTAG adapter can't drive SRST.
     */
    if (req_tlr_or_trst) {
        if (!(jtag_reset_config & RESET_HAS_TRST))
            trst_with_tlr = 1;
        else if ((jtag_reset_config & RESET_TRST_PULLS_SRST) != 0
             && !req_srst)
            trst_with_tlr = 1;
        else
            new_trst = 1;
    }

    /* Maybe change TRST and/or SRST signal state */
    if (jtag_srst != new_srst || jtag_trst != new_trst) {
        int retval;

        retval = interface_jtag_add_reset(new_trst, new_srst);
        if (retval != ERROR_OK)
            set_error(retval);
        else
            retval = execute_queue();

        if (retval != ERROR_OK) {
            LOG_ERROR("TRST/SRST error");
            return;
        }
    }

    /* SRST resets everything hooked up to that signal */
    if (jtag_srst != new_srst) {
        jtag_srst = new_srst;
        if (jtag_srst) {
            LOG_DEBUG("SRST line asserted");
            if (adapter_nsrst_assert_width)
                add_sleep(adapter_nsrst_assert_width * 1000);
        } else {
            LOG_DEBUG("SRST line released");
            if (adapter_nsrst_delay)
                add_sleep(adapter_nsrst_delay * 1000);
        }
    }

    /* Maybe enter the JTAG TAP_RESET state ...
     *  - using only TMS, TCK, and the JTAG state machine
     *  - or else more directly, using TRST
     *
     * TAP_RESET should be invisible to non-debug parts of the system.
     */
    if (trst_with_tlr) {
        LOG_DEBUG("JTAG reset with TLR instead of TRST");
        add_tlr();

    } else if (jtag_trst != new_trst) {
        jtag_trst = new_trst;
        if (jtag_trst) {
            LOG_DEBUG("TRST line asserted");
            //tap_set_state(TAP_RESET);
            if (jtag_ntrst_assert_width)
                add_sleep(jtag_ntrst_assert_width * 1000);
        } else {
            LOG_DEBUG("TRST line released");
            if (jtag_ntrst_delay)
                add_sleep(jtag_ntrst_delay * 1000);
        }
    }
}

void jtag_tap::add_sleep(uint32_t us)
{

}

void jtag_tap::add_ir_scan(struct scan_field *in_fields, tap_state_t state)
{
	assert(state != TAP_RESET);

	if (jtag_verify && jtag_verify_capture_ir)
	{
		/* 8 x 32 bit id's is enough for all invocations */

		/* if we are to run a verification of the ir scan, we need to get the input back.
		 * We may have to allocate space if the caller didn't ask for the input back.
		 */
		in_fields->check_value = this->expected;
		in_fields->check_mask = this->expected_mask;
		jtag_add_scan_check(this, jtag_add_ir_scan_noverify_callback, 1, in_fields,
							state);
	}
	else
		add_ir_scan_noverify(in_fields, state);
}

void jtag_tap::add_ir_scan_noverify(const struct scan_field *in_fields,
									tap_state_t state)
{
	jtag_prelude(state);

	int retval = interface_jtag_add_ir_scan(this, in_fields, state);
	set_error(retval);
}

void jtag_tap::add_dr_scan(int in_num_fields, const struct scan_field *in_fields, tap_state_t state)
{
	assert(state != TAP_RESET);

	jtag_prelude(state);

	int retval;
	retval = interface_jtag_add_dr_scan(this, in_num_fields, in_fields, state);
	set_error(retval);
}

void jtag_tap::add_plain_dr_scan(int num_bits, const uint8_t *out_bits, uint8_t *in_bits,
	tap_state_t state)
{
	assert(out_bits);
	assert(state != TAP_RESET);

	jtag_prelude(state);

	int retval;
	retval = interface_jtag_add_plain_dr_scan(num_bits, out_bits, in_bits, state);
	set_error(retval);
}

void jtag_tap::set_error(int error)
{
	if ((error == ERROR_OK) || (jtag_error != ERROR_OK))
		return;
	jtag_error = error;
}

int jtag_tap::execute_queue(void)
{
	int ret;
    struct list_head *h;

    h = queue_head();
    if (list_empty(h))
	    return 0;

	ret = _adapter->execute_queue(h);
    cmd_queue_free();

	return ret;
}

void jtag_tap::add_tlr(void)
{
	jtag_prelude(TAP_RESET);
	set_error(interface_jtag_add_tlr());

	/* NOTE: order here matches TRST path in jtag_add_reset() */
	//jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
	//jtag_notify_event(JTAG_TRST_ASSERTED);
}


int jtag_tap::jtag_examine_chain_execute(uint8_t *idcode_buffer, unsigned num_idcode)
{
	struct scan_field field;

	field.num_bits = num_idcode * 32;
	field.out_value = idcode_buffer;
	field.in_value = idcode_buffer;

	/* initialize to the end of chain ID value */
	for (unsigned i = 0; i < num_idcode; i++)
		buf_set_u32(idcode_buffer, i * 32, 32, END_OF_CHAIN_FLAG);

	add_plain_dr_scan(field.num_bits, field.out_value, field.in_value, TAP_DRPAUSE);
	add_tlr();
	return execute_queue();
}

int jtag_tap::jtag_init_inner()
{
	uint8_t idcode_buffer[4];
    unsigned bit_count = 0;
    int ret;

	add_tlr();
    jtag_examine_chain_execute(idcode_buffer, 1);

	uint32_t id = buf_get_u32(idcode_buffer, bit_count, 32);

	if (id & 1)
	{
        this->_idcode = id;
		chain_display(id);
	}

	return 0;
}

int jtag_tap::reset()
{
	add_reset(0, 0);
	add_reset(1, 1);

	return execute_queue();
}

uint32_t jtag_tap::idCode(bool refresh)
{
    return this->_idcode;
}

void jtag_tap::chain_display(uint32_t idcode)
{
    LOG_INFO("IDCODE: %X", idcode);
}

void jtag_tap::jtag_add_runtest(int num_cycles, tap_state_t state)
{
	jtag_prelude(state);
	set_error(interface_jtag_add_runtest(num_cycles, state));
}

/****************************************************/
void jtag_add_ir_scan(struct jtag_tap *active, struct scan_field *in_fields, tap_state_t state)
{
	if (!active)
	{
		return;
	}

	active->add_ir_scan(in_fields, state);
}

void jtag_add_dr_scan(struct jtag_tap *active, int in_num_fields, const struct scan_field *in_fields, tap_state_t state)
{
	if (!active)
	{
		return;
	}

	active->add_dr_scan(in_num_fields, in_fields, state);
}

int jtag_execute_queue(struct jtag_tap *active)
{
	if (!active)
	{
		return -1;
	}

	return active->execute_queue();
}

int jtag_init(struct jtag_tap *active)
{
	if (!active)
	{
		return -1;
	}

	return 0;
}

void jtag_add_runtest(struct jtag_tap *active, int num_cycles, tap_state_t state)
{
	if (!active)
	{
		return;
	}

	active->jtag_add_runtest(num_cycles, state);
}
